#include "track_unitConfidence.h"
#include <math.h>
#include <stdio.h>
// system
#include <float.h>
#include <string.h> 

#include "gtrack_hough.h"
#include "gtrack_int.h"
#include "typedef.h"

#define INIT_QUALITY       0.5f    // 初始质量分
#define FAST_CONFIRM_THRES 0.9f    // 快速确认运动匹配度
#define VEL_THRESHOLD      2.5f    // 速度突变检测阈值(m/s)
#define VEL_THR_X          1.0f    // 横向偏移速度

void initMotionWindow(MotionWindow* ma)
{
    ma->write_pos = 0;
    ma->valid_count = 0;
    ma->sum = 0.0;
    memset(ma->buffer, 0, sizeof(ma->buffer));  // 可选：初始化为0
}

// 当新轨迹创建时的初始化
void track_init(TrackedObject* track)
{
    track->state = STATE_TRACK_INIT;
    
    track->quality.quality_score = INIT_QUALITY;
    track->quality.last_score = INIT_QUALITY;
    track->quality.motion_consistency = 0.0f;

    track->counters.consecutive_misses = 0;
    track->counters.consecutive_hits = 0;
    track->counters.age = 0;
    track->counters.total_hits = 0;

    // memset 仅在初值赋为0时可对float类型初始化，否则不应使用，非0值应使用for循环赋值
    memset(&track->kinematics.current_center, 0, sizeof(GTRACK_measurementUnion));
    memset(&track->kinematics.optimized_result, 0, sizeof(GTRACK_measurementUnion));

    initMotionWindow(&(track->kinematics.velocity_x));
    initMotionWindow(&(track->kinematics.velocity_y));
    initMotionWindow(&(track->kinematics.position_x));
    initMotionWindow(&(track->kinematics.position_y));
    track->kinematics.centroid_snr = 0.0f;
    track->kinematics.hit_points_num = 0;

    track->velocity.filtered_vel_x = 0.0f;
    track->velocity.filtered_vel_y = 0.0f;
    track->velocity.current_vel_x  = 0.0f;
    track->velocity.current_vel_y  = 0.0f;
}

void addData(MotionWindow* ma, float newData)
{
    if (ma->valid_count < TRACK_WINDOW_SIZE)
    {
        // 数组未满：追加数据并更新总和
        ma->buffer[ma->write_pos] = newData;
        ma->sum += newData;
        ma->valid_count++;
    }
    else 
    {
        // 数组已满：替换旧数据并更新总和
        ma->sum -= ma->buffer[ma->write_pos];  // 移除旧数据
        ma->sum += newData;                // 加入新数据
        ma->buffer[ma->write_pos] = newData;   // 覆盖旧值
    }
    // 循环索引（确保在0-4之间）
    ma->write_pos = (ma->write_pos + 1) % TRACK_WINDOW_SIZE;
}

void track_update_kinematics_(TrackedObject* obj, GTRACK_measurementPoint cur_center, GTRACK_measurementUnion optimized_result, float vel_x, float vel_y)
{
    addData(&obj->kinematics.velocity_x, vel_x);//添加Vx
    addData(&obj->kinematics.velocity_y, vel_y);//添加Vy 
    GTRACK_cartesian_position cur_pose;
    gtrack_sph2cart(&cur_center.vector, &cur_pose);
    addData(&obj->kinematics.position_x, cur_pose.posX);//添加Px
    addData(&obj->kinematics.position_y, cur_pose.posY);//添加Py 
    obj->velocity.current_vel_x = vel_x;
    obj->velocity.current_vel_y = vel_y;

    obj->kinematics.centroid_snr = cur_center.snr;
    obj->kinematics.current_center.vector = cur_center.vector; 
    obj->kinematics.optimized_result = optimized_result;
}

float getAverage(MotionWindow* ma)
{
    if (ma->valid_count == 0)
    {
        return 0.0;  // 无数据时返回0（或自定义错误处理）
    }
    return ma->sum / (ma->valid_count < TRACK_WINDOW_SIZE ? ma->valid_count : TRACK_WINDOW_SIZE);
}

// 匹配度得分 返回0-1的匹配度（基于卡尔曼滤波器）
void update_motion_consistency(void* handle)
{
    GtrackUnitInstance* inst;
    inst = (GtrackUnitInstance*)handle;
    // 计算预测与实测的马氏距离
    //GTRACK_measurementUnion u_tilda;
    float dist;
    //gtrack_vectorsub(gtrack_measurement_vector_size, inst->track_state.test_value.cur_center.array, inst->track_state.test_value.cur_optimizedresults.array, u_tilda.array);
    //gtrack_computemahalanobis(u_tilda.array, inst->gc_inv, &dist);
    dist = gtrack_calcDistance(&inst->track_state.kinematics.current_center.vector,&inst->track_state.kinematics.optimized_result.vector);
    inst->track_state.quality.motion_consistency = expf(-dist * dist / 9.0f);    // 非线性映射到匹配度
}

// 质量得分 
float calculate_quality_score(void* handle)
{
    GtrackUnitInstance* inst;
    inst = (GtrackUnitInstance*)handle;
    // 检测稳定性：连续命中次数的非线性映射
    float hit_factor = 1.0f - expf(-inst->track_state.counters.consecutive_hits / 3.0f);
    //printf("命中分：%f\n", hit_factor);
    // 运动连续性：最新两次预测的匹配度
    update_motion_consistency(inst);
    float motion_match = inst->track_state.quality.motion_consistency;
    //质量分 = 检测稳定性(40 %) + 运动连续性(40 %) + 考虑20 % 的历史得分因素
    return 0.4f * hit_factor + 0.4f * motion_match + 0.2f * inst->track_state.quality.quality_score;
}

// 最终得分 
void update_quality_score(void* handle) 
{ 
    GtrackUnitInstance* inst;
    inst = (GtrackUnitInstance*)handle; 
    if (inst->track_state.state == STATE_TRACK_INIT)
        return;
    // 计算新的质量分（基于三个维度维指标）
    float new_score = calculate_quality_score(inst);
    // 滑动平均更新：保留历史信息的80% + 当前帧的20%
    inst->track_state.quality.quality_score = 0.8f * inst->track_state.quality.quality_score + 0.2f * new_score;
}

// 动态阈值生成函数（存活越久阈值越高）
int get_adaptive_release_threshold(TrackedObject* track)
{
    const int BASE_THRESH = 3;       // 基础阈值
    const float AGE_FACTOR = 0.1f;   // 生存时间增益

    // 阈值 = 基础值 + 生存时间增益
    return BASE_THRESH + (int)(MIN(track->counters.age * AGE_FACTOR,  10));
}
bool Track_IsMotionStableAfterHits(TrackedObject* track)
{
    const float HIT_RATE = 0.6f;  // 先随便定义一个比例 也不是随便 应该等于想要检测的帧数但是没有连续命中x（暂时是3）次，又大部分命中的比例
    bool CON1 = (track->counters.consecutive_hits >= 5) && (track->quality.motion_consistency > 0.9f);//连续三个都跟的好 但是这是个顺时的，好像也不对
    bool CON2 = track->counters.age > 5 && ((float)track->counters.total_hits / track->counters.age > HIT_RATE); //约等于11次命中8次 条件苛刻
    return CON1 || CON2;
}

bool Track_HasQualityScoreSurge(TrackedObject* track)
{
    return track->quality.quality_score > track->quality.last_score * 1.2f;
    //return track->quality.quality_score > 0.6;
}

bool Track_IsVelocityWithinThreshold(TrackedObject* track)
{
    track->velocity.filtered_vel_x = getAverage(&track->kinematics.velocity_x);//更新当前平均
    track->velocity.filtered_vel_y = getAverage(&track->kinematics.velocity_y);//更新当前平均
    float temp = 0.0f;
    for (int i = 0; i < track->kinematics.velocity_y.valid_count; ++i)
    {
        temp += track->kinematics.velocity_y.buffer[i] * track->kinematics.velocity_y.buffer[i];
    }
    temp = temp / track->kinematics.velocity_y.valid_count - track->velocity.filtered_vel_y * track->velocity.filtered_vel_y;
    temp = sqrtf(temp);         // 标准差
    temp = MAX(0.01f, temp);    // 避免float误差导致标准差为0

    bool con1 = track->velocity.current_vel_x < VEL_THR_X;
    bool con2 = track->velocity.filtered_vel_x < VEL_THR_X;
    bool con3 = fabsf(track->velocity.current_vel_y - track->velocity.filtered_vel_y) < 3 * temp;//差别小于3倍标准差，要求纵向速度变化不大
    //bool con4 = houghDetection(&track->kinematics.position_x, &track->kinematics.position_y);
    bool con4 = true;
    return con1 && con2 && con3 && con4;
}

// 当满足以下条件之一时立即确认轨迹
bool check_fast_confirm(TrackedObject* track)
{
    // 条件1：前3帧连续命中且运动匹配度>0.9
    bool cond1 = Track_IsMotionStableAfterHits(track);
    // 条件2：质量分突增（差分检测）
    bool cond2 = Track_HasQualityScoreSurge(track);
    //条件三 速度无突变
    bool cond3 = Track_IsVelocityWithinThreshold(track);
    //printf("此时，判断状态 ：%d-%d-%d \n", cond1, cond2, cond3);
    return (cond1 || cond2) && cond3;
}

void update_track_state(void* handle)
{
    GtrackUnitInstance* inst;
    inst = (GtrackUnitInstance*)handle;
    switch (inst->track_state.state)
    {
    case STATE_TRACK_INIT:
        inst->track_state.state = STATE_TRACK_TENTATIVE;
        break;
    case STATE_TRACK_TENTATIVE:
        if (check_fast_confirm(&inst->track_state))
        {
            inst->track_state.state = STATE_TRACK_CONFIRMED;
        }
        else if (inst->track_state.quality.quality_score < 0.3f || inst->track_state.counters.consecutive_misses > 2)
        {
            inst->track_state.state = STATE_TRACK_MISSED;
        }
        break;
    case STATE_TRACK_CONFIRMED:
        // 动态阈值释放机制
        if (inst->track_state.counters.consecutive_misses > get_adaptive_release_threshold(&inst->track_state))
        {
            inst->track_state.state = STATE_TRACK_MISSED;
        }
        break;
    }
   
    inst->track_state.quality.last_score = inst->track_state.quality.quality_score; // 记录历史分

    //打印
    int release_thresh = get_adaptive_release_threshold(&inst->track_state);
    //printf("此时，命中次数%d miss次数/阈值：%d/%d  运动得分：%f - 总得分：%f,状态 :%d\n", inst->track_state.counters.total_hits, inst->track_state.counters.consecutive_misses, release_thresh, inst->track_state.quality.motion_consistency, inst->track_state.quality.quality_score, inst->track_state.state);
}


// 每次丢失时质量分非线性下降
void apply_miss_penalty_(TrackedObject* track)
{
    //! miss后是否更容易满足质量分突增条件？
    track->quality.quality_score *= 0.7f;  // 比线性下降更陡峭
}

void track_unitConfidence_update_(void* handle)
{
    //更新当前质量分
    update_quality_score(handle);
    //状态确认
    update_track_state(handle);
}

void stateHanding_(void* handle)
{
    //测试这么搞，后面直接修改状态
    GtrackUnitInstance* inst;
    inst = (GtrackUnitInstance*)handle;
    switch (inst->track_state.state)
    {
        case STATE_TRACK_CONFIRMED:
            inst->state = TRACK_STATE_ACTIVE; break;
        case STATE_TRACK_MISSED:
            inst->state = TRACK_STATE_FREE; break;
        default:break;
    }
}

void gtrack_unitConfidence(void* handle, uint16_t num)
{
    GtrackUnitInstance* inst;
    inst = (GtrackUnitInstance *)handle;

    //更新age 应该等于 age++
    inst->track_state.counters.age = inst->heartBeatCount - inst->allocationTime + 1;
    inst->track_state.kinematics.hit_points_num = num;// 记录命中次数
    if (num)
    {
        track_update_kinematics_(&inst->track_state, inst->center, inst->H_s, inst->S_hat[2], inst->S_hat[3]);
        if (num > inst->allocationParams->pointsThre)
        {
            inst->track_state.counters.total_hits += 1; //给与更高评价 test
        }
        else
        {
            inst->track_state.counters.total_hits += 1; //正常命中+1
        }
        inst->track_state.counters.consecutive_hits++;
        inst->track_state.counters.consecutive_misses = 0;
    }
    else/* Miss */
    {
        inst->track_state.counters.consecutive_misses++;
        apply_miss_penalty_(&inst->track_state); //对miss的情况进行得分惩罚
        inst->track_state.counters.consecutive_hits = 0;
    }
    track_unitConfidence_update_(inst);

    //对轨迹状态的处理
    stateHanding_(inst);
}

// 检测速度矢量的突然变化（需维护速度历史）
//bool detect_sudden_move(Track_STATE_PARAM* track) 
//{
//    float current_vel = track->motion_state.cur_vel;
//    float avg_vel = track->motion_state.ave_vel; // 滑动平均
//
//    // 计算速度变化率
//    float vel_diff = fabsf(current_vel - avg_vel);
//    return vel_diff > VEL_THRESHOLD;
//}

// 当多个轨迹竞争同一检测时增强优胜者
//void boost_winner_track(Track_STATE_PARAM* winner) 
//{
//    winner->quality_score += 0.5f;  // 奖励优胜者
//    winner->hits += 2;              // 加速确认
//}


/**
 * 更新轨迹得分（关联成功时）
 * @param track     轨迹对象
 * @param PD        检测概率（0.9）
 * @param PG        门限概率（0.95）
 * @param lambda_FA 虚警密度（0.1）
 * @param lambda_NT 新生目标密度（0.01）
 */
void update_score_on_association(void* handle, float PD, float PG, float lambda_FA, float lambda_NT)
{}

/**
 * 更新轨迹得分（关联失败时）
 * @param track     轨迹对象
 * @param PD        检测概率
 * @param PG        门限概率
 */
void update_score_on_miss(void* handle, float PD, float PG) {}


